#ifdef __cplusplus
extern "C"{
#endif

#include "CAN_cmd_damiao.h"
#include "CAN_cmd_dji.h"
#include "CAN_cmd_lingkong.h"
#include "pid.h"

#ifdef __cplusplus
}
#endif


#include "port.hpp"

#ifndef CLIO_MECHANICAL_ARM_HPP
#define CLIO_MECHANICAL_ARM_HPP

using namespace std;

class MOTOR{

public: 

uint8_t gear_ratio;
uint8_t can_bus;
uint8_t can_id;
float expect_pos;
float expect_vel;
float real_vel;
float real_pos;

struct Limit{
float max_vel;
float max_acc;
float max_pos;
float min_pos;
uint8_t error_flag;
};

Limit limit;

pid_type_def pid;

InputPort<float> expect_pos_port;
InputPort<float> real_vel_port;
InputPort<float> real_pos_port;


void gearratio_and_can_init(uint8_t gear_ratio_index,uint8_t can_bus_index,uint8_t can_id_index)
{
gear_ratio=gear_ratio_index;
can_bus=can_bus_index;
can_id=can_id_index,
};

void limit_init(float max_vel_index,float max_acc_index,float max_pos_index,float min_posv)
{
limit.max_vel=max_vel_index;
limit.max_acc=max_acc_index;
limit.max_pos=max_pos_index;
limit.min_pos=min_posv;
};

void pid_init(uint16_t kp_index,uint16_t ki_index,uint16_t kd_index,float max_out_index,float max_iout_index)
{
pid.Kp=kp_index;
pid.Ki=ki_index;
pid.Kd=kd_index;
pid.max_out=max_out_index;
pid.max_iout=max_iout_index;
};

 void sensor_data_update(){
real_pos=real_pos_port.present()  ;
real_vel=real_vel_port.present();
real_vel=real_vel/gear_ratio;
// expect_pos=expect_pos_port.present();
 };

virtual void caculate_and_output(){};
virtual void error_stop(){};

};

/**校准回零、映射模式、自动模式、无力模式 */

class MOTOR_DAMIAO:public MOTOR
{



virtual void caculate_and_output() override
{
  PID_calc()
};
};






class MOTOR_LINGKONG:public MOTOR
{


virtual void caculate_and_output() override
{
};


};


class MOTOR_DJI:public MOTOR
{


virtual void caculate_and_output() override
{
};


};






/**初始化、状态机切换、状态循环 */

class MOTOR;
extern std::vector<std::unique_ptr<MOTOR>> motors;




















#endif













